#ifndef EXECUTOR_H
#define EXECUTOR_H

#include <cmath>
#include <iostream>
#include <libplayerc++/playerc++.h>
#include "pause.h"
#include "VelocityTuning.h"
#include "PlayerObjectManager.h"
#include "PeopleCircle.h"
#include "GlobalStructure.h"

class Executor {

  const std::vector<Position>& path;
  PlayerCc::PlayerClient & robot;
  PlayerCc::Position2dProxy & pp;
  PlayerCc::Position2dProxy & gocon;

  bool at_position(player_pose2d pose) {
    // TODO: use spaceinformation?
    return (fabs(pose.px - pp.GetXPos()) + fabs(pose.py - pp.GetYPos())) < 0.1;
  }

public:
  Executor(const std::vector<Position>& path,
    PlayerCc::PlayerClient & robot,
    PlayerCc::Position2dProxy & pp,
    PlayerCc::Position2dProxy & gocon)
  : path(path), robot(robot), pp(pp), gocon(gocon) {
  }

  // execute_plan()
    void execute() {
        std::size_t stateCount = path.size();
        VelocityTuning velTuner(0.2);
        Robot2D current;
        double p = pow(10.0,9.0);

        PeopleCircle::Instance().RefreshPositions();
        
        std::vector<PlayerObject>& obstacles = PlayerObjectManager::Instance().GetDynamicObjects();

        for (int i = 1; i < stateCount; i++) {
            const Position& state = path[i];
            player_pose2d waypoint;
            waypoint.px = state.x;
            waypoint.py = state.y;
            waypoint.pa = state.yaw;
           
            PeopleCircle::Instance().RefreshPositions();

            current.x = pp.GetXPos();
            current.y = pp.GetYPos();
            std::cout << "From: " << pp.GetXPos() << "," << pp.GetYPos() << endl;
            std::cout << "Going to (" << waypoint.px << ", " << waypoint.py << ")\n";
            Robot2D nextPos (waypoint.px,waypoint.py);
            Segment2D segpath (current, nextPos);

            velTuner.updateState(current, segpath, obstacles);
            std::pair<double, double> sol = velTuner.getSol();
            if (sol.first > 0) {
                pp.SetSpeed(0,0,0);
                long sec = (long)sol.first;
                double nv_sec = sol.first - sec;
                util::pause((long)sol.first, (long)(nv_sec * p));
            }
            if (sol.first == 0 && sol.second == 0) {
                 pp.SetSpeed(0,0,0);
                util::pause(5,0);
                continue;
            }
            if (!at_position(waypoint)) {
                // if not at position, send the goto command
//                gocon.GoTo(waypoint);
                Robot2D pos = current;
                int i = 0;
                do {
                    //10 minisecond
                    i++;
                    util::pause(0,1000000);
                    robot.Read();
                    pos.x = pp.GetXPos();
                    pos.y = pp.GetYPos();
                    Robot2D speed = velTuner.getVelocity(i/1000.0);
//                    std::cout << "At position: "<<pp.GetXPos() << " , " << pp.GetYPos() << std::endl;
//                    std::cout << "At speed: "<< speed.x << " , " << speed.y<<" , "<<speed.yaw << std::endl;

                    pp.SetSpeed(speed.x,speed.y,0.0);
                } while (!at_position(waypoint));
//                pp.SetSpeed(0,0,0);
            }
        }
        pp.SetSpeed(0,0,0);
    }
};

#endif
